#include <math.h>
#include <stdio.h>
#include "constants.h"
#include "geodesy.h"








int PerformClosedFormPositionSolution_FromPseuodrangeMeasurements(
  double p1,          //pseudoranges    
  double p2,         
  double p3,        
  double p4,       
  double prc_satclk1, //clock correction
  double prc_satclk2,
  double prc_satclk3,
  double prc_satclk4,
  double x1,          //sat coord
  double x2,         
  double x3,        
  double x4,       
  double y1,      
  double y2,     
  double y3,    
  double y4,   
  double z1,  
  double z2, 
  double z3,
  double z4,
  double* latitude,   //result 
  double* longitude, 
  double* height     
  )
{
  // difference values
  double x12, x13, x14; // 'xij' = 'xi' - 'xj' [m]
  double y12, y13, y14;
  double z12, z13, z14;
  double p21, p31, p41; // 'pij' = 'pi' - 'pj' [m]

  double k1, k2, k3; // coefficients
  double c1, c2, c3;
  double f1, f2, f3;
  double m1, m2, m3;

  double d1;   // clock bias, solution 1 [m]
  double d2;   // clock bias, solution 2 [m]
  double detA; // determinant of A
  double tmp1;
  double tmp2;
  double tmp3;
  double A[3][3];
  double D[3][3]; // D is A^-1 * detA

  typedef struct
  {
    double x;
    double y;
    double z;    
  } struct_SOLN;

  struct_SOLN s1; 
  struct_SOLN s2;
  // apply the satellite clock corrections
  p1 = p1 + prc_satclk1;
  p2 = p2 + prc_satclk2;
  p3 = p3 + prc_satclk3;
  p4 = p4 + prc_satclk4;  
  
  x12 = x1 - x2;
  x13 = x1 - x3;
  x14 = x1 - x4;

  y12 = y1 - y2;
  y13 = y1 - y3;
  y14 = y1 - y4;
  
  z12 = z1 - z2;
  z13 = z1 - z3;
  z14 = z1 - z4;

  p21 = p2 - p1;
  p31 = p3 - p1;
  p41 = p4 - p1;

  k1 = x12*x12 + y12*y12 + z12*z12 - p21*p21;
  k2 = x13*x13 + y13*y13 + z13*z13 - p31*p31;
  k3 = x14*x14 + y14*y14 + z14*z14 - p41*p41;

  A[0][0] = 2.0*x12;
  A[1][0] = 2.0*x13;
  A[2][0] = 2.0*x14;

  A[0][1] = 2.0*y12;
  A[1][1] = 2.0*y13;
  A[2][1] = 2.0*y14;

  A[0][2] = 2.0*z12;
  A[1][2] = 2.0*z13;
  A[2][2] = 2.0*z14;

  tmp1 = A[1][1]*A[2][2] - A[2][1]*A[1][2];
  tmp2 = A[1][0]*A[2][2] - A[2][0]*A[1][2];
  tmp3 = A[1][0]*A[2][1] - A[2][0]*A[1][1];

  detA = A[0][0]*tmp1 - A[0][1]*tmp2 + A[0][2]*tmp3;

  D[0][0] =  tmp1;
  D[1][0] = -tmp2;
  D[2][0] =  tmp3;

  D[0][1] = -A[0][1]*A[2][2] + A[2][1]*A[0][2];
  D[1][1] =  A[0][0]*A[2][2] - A[2][0]*A[0][2];
  D[2][1] = -A[0][0]*A[2][1] + A[2][0]*A[0][1];

  D[0][2] =  A[0][1]*A[1][2] - A[1][1]*A[0][2];
  D[1][2] = -A[0][0]*A[1][2] + A[1][0]*A[0][2];
  D[2][2] =  A[0][0]*A[1][1] - A[1][0]*A[0][1];

  c1 = (D[0][0]*p21 + D[0][1]*p31 + D[0][2]*p41) * 2.0 / detA;
  c2 = (D[1][0]*p21 + D[1][1]*p31 + D[1][2]*p41) * 2.0 / detA;
  c3 = (D[2][0]*p21 + D[2][1]*p31 + D[2][2]*p41) * 2.0 / detA;

  f1 = (D[0][0]*k1 + D[0][1]*k2 + D[0][2]*k3) / detA;
  f2 = (D[1][0]*k1 + D[1][1]*k2 + D[1][2]*k3) / detA;
  f3 = (D[2][0]*k1 + D[2][1]*k2 + D[2][2]*k3) / detA;

  m1 = c1*c1 + c2*c2 + c3*c3 - 1.0;
  m2 = -2.0*( c1*f1 + c2*f2 + c3*f3 );
  m3 = f1*f1 + f2*f2 + f3*f3;
  tmp1 = m2*m2 - 4.0*m1*m3;
  if( tmp1 < 0 )
  {
     printf("DEBUG: no solution\n");
     return FALSE;
  }

  d1 = ( -m2 + sqrt( tmp1 ) ) * 0.5 / m1;
  d2 = ( -m2 - sqrt( tmp1 ) ) * 0.5 / m1;

  // two solutions
  s1.x = c1*d1 - f1 + x1;
  s1.y = c2*d1 - f2 + y1;
  s1.z = c3*d1 - f3 + z1;

  s2.x = c1*d2 - f1 + x1;
  s2.y = c2*d2 - f2 + y1;
  s2.z = c3*d2 - f3 + z1;

  tmp1 = sqrt( s1.x*s1.x + s1.y*s1.y + s1.z*s1.z );
  tmp2 = sqrt( s2.x*s2.x + s2.y*s2.y + s2.z*s2.z );

  // choose the correct solution based
  tmp1 = fabs( tmp1 - 6378136.0 );
  tmp2 = fabs( tmp2 - 6378136.0 );

  if( tmp2 < tmp1 )
  {
    s1 = s2;
  }


  GEODESY_ConvertEarthFixedCartesianToGeodeticCurvilinearCoordinates(
    GEODESY_REFERENCE_ELLIPSE_PZ90,
    s1.x, s1.y, s1.z,
    latitude, longitude, height );

  if( *height < -1500.0 || *height > 18000.0 )
  {
     //printf("DEBUG: height\n");
     //return FALSE;
  }
  return TRUE;
}

